#! /usr/bin/env python
# -*- coding: utf-8 -*-


#1.导包 
import rospy
import os
import sys
import time
import cv2
import threading

sys.path.insert(0,"/home/nvidia/catkin_ws/src/px4_swarm/scripts")

import VTD_communicate


from geometry_msgs.msg import Twist

if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("car_follow_message_get")
    #3.实例化 发布者 对象
    pub = rospy.Publisher("uav_follow_control",Twist,queue_size=10)
    #4.组织被发布的数据，并编写逻辑发布数据
    velocity_cmd = Twist()
    velocity_cmd.linear.x = 0.0
    velocity_cmd.linear.y = 0.0
    velocity_cmd.linear.z = 1.0
    velocity_cmd.angular.x = 0.0
    velocity_cmd.angular.y = 0.0
    velocity_cmd.angular.z = 0.0


    IP = '192.168.8.139'  # 为运行VTD_server_deploy.py电脑的IP  '10.1.251.85'
    PORT = 10100

    # 创建一个udp_s类，并绑定ip、port(端口号)，用于发图片、收信息
    udp_s = VTD_communicate.VTDUdp(IP, PORT, communicate_type='receive_mes')
    # 启用多线程，接收消息
    re_mes = threading.Thread(target=udp_s.udp_receive_uav_parameters, args=())
    re_mes.start()

    # ---------测试用----------
    start_time = time.time()
    # ---------测试用----------

    cap = cv2.VideoCapture(0)

    # 设置循环频率
    rate = rospy.Rate(100)

    while not rospy.is_shutdown():
        # try:
        ret, im = cap.read()  # 摄像头读取,ret为是否成功打开摄像头,true,false。 im为视频的每一帧图像
        # im = cv2.flip(im, 1)  # 摄像头是和人对立的，将图像左右调换回来正常显示。

        if ret:
            udp_s.udp_send_img(im)

            velocity_cmd.linear.x = udp_s.uav_parameters[0]
            velocity_cmd.linear.y = udp_s.uav_parameters[1]
            velocity_cmd.linear.z = udp_s.uav_parameters[2]
            velocity_cmd.angular.x = 0.0
            velocity_cmd.angular.y = 0.0
            velocity_cmd.angular.z = udp_s.uav_parameters[3]
            # ---------测试用----------
            # 无人机的控制参数
            # if time.time() - start_time > 3:
            #     start_time = time.time()
            #     print(udp_s.uav_parameters)
            # ---------测试用----------
        #publish
        pub.publish(velocity_cmd)
        rate.sleep()
        
